#ifndef LEGS_DATA_H
#define LEGS_DATA_H
// Characters that represent leg component positions
// First row: u (upper), l(lower)
// 2nd row: f (forward), b(back)
// 3rd row: l (left), r (right)
// 4th row: f (front), c (center), r (right)
char leg_comp_pos[4][3] = 
{
 { 'u', 'l', 'X'},
 { 'f', 'b', 'X'},
 { 'l', 'r', 'X'},
 { 'f', 'c', 'b'}
};

// Calibration data for MIAU's legs
// This array stores the position required for each servo
// to have each leg straight and at a right angle with the body.
// The sign is indicates whether to add or substract to 
// the center position to move the leg forward or back, respectively
int leg_calibration[2][2][2] = 
{
	{ 
		{ 
			-715,  // upper front left
			640 	// upper front right
		},
		{ 
			-710,	// upper back left
			750 	// upper back right
		}
	},
	{ 
		{ 
			875,	// lower front left
			-420 	// lower front right
		},
		{ 
			950,	// lower back left
			-430 	// lower back right
		}
	}
};

// Port letters corresponding to each leg component position.
uint8_t *leg_port_letters[2][2][2] =
{
	{ 
		{ 
			&PORTE,  // upper front left
			&PORTA 	// upper front right
		},
		{ 
			&PORTH,	// upper back left
			&PORTC 	// upper back right
		}
	},
	{ 
		{ 
			&PORTE,	// lower front left
			&PORTA 	// lower front right
		},
		{ 
			&PORTH,	// lower back left
			&PORTC 	// lower back right
		}
	}
};


// Port numbers corresponding to each leg component position.
int leg_port_numbers[2][2][2] =
{
	{ 
		{ 
			2,  // upper front left
			5 	// upper front right
		},
		{ 
			2,	// upper back left
			5 	// upper back right
		}
	},
	{ 
		{ 
			3,	// lower front left
			6 	// lower front right
		},
		{ 
			3,	// lower back left
			4 	// lower back right
		}
	}
};





int walking_sequence[16] [4] = {
	{ LOWER, FRONT, LEFT, SERVO_BACK},
	{ LOWER, FRONT, RIGHT, SERVO_FORWARD},
	{ UPPER, FRONT, LEFT, SERVO_FORWARD },
	{ UPPER, FRONT, RIGHT, SERVO_BACK },

	{ LOWER, BACK, LEFT, SERVO_BACK},
	{ LOWER, BACK, RIGHT, SERVO_FORWARD},
	{ UPPER, BACK, LEFT, SERVO_FORWARD },
	{ UPPER, BACK, RIGHT, SERVO_BACK },
/*
	{ LOWER, BACK, RIGHT, SERVO_BACK},
	{ LOWER, BACK, LEFT, SERVO_FORWARD},
	{ UPPER, BACK, RIGHT, SERVO_FORWARD },
	{ UPPER, BACK, LEFT, SERVO_BACK },
*/
	{ UPPER, FRONT, RIGHT, SERVO_FORWARD },
	{ UPPER, FRONT, LEFT, SERVO_BACK },
	{ LOWER, FRONT, RIGHT, SERVO_BACK},
	{ LOWER, FRONT, LEFT, SERVO_FORWARD},

	{ UPPER, BACK, RIGHT, SERVO_FORWARD },
	{ UPPER, BACK, LEFT, SERVO_BACK },
	{ LOWER, BACK, RIGHT, SERVO_BACK},
	{ LOWER, BACK, LEFT, SERVO_FORWARD},

/*
	{ UPPER, BACK, LEFT, SERVO_FORWARD },
	{ UPPER, BACK, RIGHT, SERVO_BACK },
	{ LOWER, BACK, LEFT, SERVO_BACK},
	{ LOWER, BACK, RIGHT, SERVO_FORWARD}
*/


};

int positions[3] = { 200, 0, -200};

int current_leg_commands[8][5];
#endif
